//
// Created by 16933 on 2022/5/2.
//

#include "Calculate.h"
#include "cmsis_os.h"
#include "inv_mpu.h"
#include "tim.h"
#include "usart.h"
#include "bsp_Motor.h"
#include "Pid.h"
#include "bsp_MPU6050.h"
float pitch;
float roll;
float yaw;
short gx;
short gy;
short gz;
extern float offset;
int offset_flag;
motor_measure_t Motor_Left;   //左边电机
motor_measure_t Motor_Right;  //右边电机

pid_t PID_Motor_Left_Speed;    //左边电机速度环pid控制器
pid_t PID_Motor_Right_Speed;   //右边电机速度环pid控制器
pid_t PID_Motor_Angle;    //左边电机角度环pid控制器
pid_t PID_Motor_Speed;


float PID_Speed_Lout;    //左边电机速度环pid输出结果
float PID_Speed_Rout;    //右边电机速度环pid输出结果
float PID_Angle_Out;    //角度环pid输出结果
float PID_Speed_Out;
float speed_kp = 4.25;       //3.5


extern uint8_t MpuResetFlag;
extern uint8_t MpuResetCompleteFlag;
uint8_t Wifi_Buffer[40];
uint8_t BlueToothBuffer[1];
float target_angle = 0;
int encoder_Data = 0;
int duibi = 0;
void CalculateTask(void const * argument)
{
    HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_4);
    /*  pid控制器初始化   */
    //速度环
    pid_init(&PID_Motor_Left_Speed,500,350,50,25,0,60);  //左电机
    pid_init(&PID_Motor_Right_Speed,500,350,55,28,0,60);  //右电机
    // 角度环
    pid_init(&PID_Motor_Angle,30,30,0.680,0,-0.00195,0);
                                                                //0.8          -0.003       -0.0021
    pid_init(&PID_Motor_Speed,90,90,speed_kp,speed_kp/150,0,0);
                                                                // 4
//    pid_init(&PID_Motor_Angle,30,30,1.15,0,5000,0);
    uint32_t calculate_time = osKernelSysTick();
    for(;;)
    {
//        HAL_GPIO_WritePin(TEST_GPIO_Port,TEST_Pin,GPIO_PIN_SET);
        if(Wifi_Buffer[0]!=0)
        {
            int i = 0;
            do {
                i++;
            } while (Wifi_Buffer[i]!=':');
            if(Wifi_Buffer[i]==1)
            {
                target_angle = 5.0;
            }
        }
        //左边电机速度、圈数计算
        encoder_data_handle(&Motor_Left,__HAL_TIM_GET_COUNTER(&htim2));
        //右边电机速度、圈数计算
        HAL_Delay(1);
        encoder_data_handle(&Motor_Right,__HAL_TIM_GET_COUNTER(&htim4));
        mpu_dmp_get_data(&pitch,&roll,&yaw);
        MPU_Get_Gyroscope(&gx,&gy,&gz);
        if(pitch>60 || pitch<-60 || MpuResetCompleteFlag==0 )
        {
            MpuResetCompleteFlag = 0;
            MpuResetFlag = 1;
            pitch = 0;
            roll = 0;
            yaw = 0;
            gx = 0;
            gy = 0;
            gz = 0;
            PID_Angle_Out = 0;
            PID_Speed_Lout = 0;
            PID_Speed_Rout = 0;
            pid_reset(&PID_Motor_Left_Speed,50,25,0);  //左电机
            pid_reset(&PID_Motor_Right_Speed,55,28,0);  //右电机
            // 角度环
            pid_reset(&PID_Motor_Angle,0.62,0.038,2);
        }
        else if (MpuResetFlag==0 && MpuResetCompleteFlag==1)
        {
            MpuResetFlag = 0;
//            if (pitch-offset>8 || pitch-offset<-8)
//            {
//                PID_Motor_Angle.p = 0.8;
//            } else
//            {
//                PID_Motor_Angle.p = 0.62;
//            }
            encoder_Data = encoder_filter(Motor_Left.total_ecd_delta,Motor_Right.total_ecd_delta);
            duibi = Motor_Left.total_ecd_delta+Motor_Right.total_ecd_delta;
            PID_Speed_Out = pid_calc(&PID_Motor_Speed,(float)(encoder_Data/2.0),0);
            PID_Angle_Out = -pid_calc_ang(&PID_Motor_Angle,pitch-offset,PID_Speed_Out,gy);
            PID_Speed_Lout = pid_calc(&PID_Motor_Left_Speed,Motor_Left.speed_rpm,PID_Angle_Out);
            PID_Speed_Rout = pid_calc(&PID_Motor_Right_Speed,Motor_Right.speed_rpm,PID_Angle_Out);
        }




        osDelayUntil(&calculate_time,5);
    }

}
